#include <cassert>
#include <iostream>

#include <GL/gl.h>
#include "math/vec3.h"
#include "model/ms3d.h"

void slPrintMatrix(const slMat4* pMat)
{
	for (int i = 0; i < 4; ++i)
		std::cout << pMat->m_Mat[i] << " " << pMat->m_Mat[i+4] << " " << pMat->m_Mat[i+8] << " " << pMat->m_Mat[i+12] << std::endl;
}

void slPrintVector(const slVec3* pVec)
{
	std::cout << "(" << pVec->m_X << ", " << pVec->m_Y << ", " << pVec->m_Z << ")" << std::endl;
}
namespace sheeplib {

    namespace model {

		const int MS3D_MAX_VERTICES = 8192;
		const int MS3D_MAX_TRIANGLES = 16384;
		const int MS3D_MAX_GROUPS = 128;
		const int MS3D_MAX_MATERIALS = 128;
		const int MS3D_MAX_JOINTS = 128;
		const int MS3D_MAX_KEYFRAMES = 216;

		enum MS3DFlags {
			SELECTED = 1,
			HIDDEN,
			SELECTED2,
			DIRTY
		};

        MS3D::MS3D() {

        }

        MS3D::~MS3D() {

        }

		/// Reads the first part of the model file, and checks that the header is valid
		bool MS3D::CheckFileHeaderIsValid(ifstream& fileIn) {
			MS3DHeader header;
			//Read the header
			fileIn.read(reinterpret_cast < char * > (&header), sizeof(MS3DHeader));

			string ID = string(header.id);
			short version = header.version;

			//Check the version
			if (ID.find("MS3D000000") == string::npos || (version < 3 && version > 7))
				return false; //Invalid format / file

			return true; //Version and ID are OK.
		}

		///Read the vertices from the file
		bool MS3D::ReadVertices(ifstream& fileIn, vector<MS3DVertex>& vertices, word numVertices) {
			assert(numVertices > 0); //Check that numVertices is positive
			assert(fileIn.good()); //Check our file is OK

			//Create space for the vertices
			vertices.resize(numVertices);
			//Read the vertices into the new array
			fileIn.read(reinterpret_cast<char*>(&vertices[0]),
									sizeof(MS3DVertex) * numVertices);
			return true;
		}

		bool MS3D::ReadTriangles(ifstream& fileIn, vector<MS3DTriangle>& triangles, word numTriangles) {
			assert(numTriangles > 0);
			assert(fileIn.good());

			triangles.resize(numTriangles);

			for (int i = 0; i < numTriangles; ++i) {
				fileIn.read(reinterpret_cast<char*>(&triangles[i]), sizeof(MS3DTriangle));
			}
			return true;
		}

		bool MS3D::ReadGroups(ifstream& fileIn, vector<MS3DGroup>& groups, word numGroups) {
			assert(numGroups > 0);
			assert(fileIn.good());

			//Create space for the groups
			groups.resize(numGroups);
			//and load them
			for (word i = 0; i < numGroups; ++i) {
				fileIn.read(reinterpret_cast<char*>(&groups[i].m_Flag), sizeof(byte));
				fileIn.read(reinterpret_cast<char*>(&groups[i].m_Name), sizeof(char) * 32);
				fileIn.read(reinterpret_cast<char*>(&groups[i].m_NumTriangles), sizeof(word));

				assert(groups[i].m_NumTriangles > 0);

				groups[i].m_TriangleIndices.resize(groups[i].m_NumTriangles);

				for (int j = 0; j < groups[i].m_NumTriangles; ++j) {
					fileIn.read(reinterpret_cast < char * > (&groups[i].m_TriangleIndices[j]), sizeof(word));
				}

				fileIn.read(reinterpret_cast < char * > (&groups[i].m_MaterialIndex), sizeof(char));
			}

			return true;
		}

		bool MS3D::ReadMaterials(ifstream& fileIn, vector<MS3DMaterial>& materials, word numMaterials) {
			assert(numMaterials > 0);
			assert(fileIn.good());
			//Create space for the groups
			materials.resize(numMaterials);
			//and load them
			for (int i = 0; i < numMaterials; ++i) {
				fileIn.read(reinterpret_cast < char * > (&materials[i]), sizeof(MS3DMaterial));
			}

			return true;
		}

		bool MS3D::ReadJoints(ifstream& fileIn, vector<MS3DJoint>& joints, word numJoints) {
			assert(fileIn.good());

			if (numJoints == 0) //Some files might not have joints so don't raise an assertion
				return true;

			joints.resize(numJoints);

			for (int i = 0; i < numJoints; ++i) {
				//First read in the flags
				fileIn.read(reinterpret_cast < char * > (&joints[i].m_Flags), sizeof(byte));
				//then the joint name (32 chars)
				fileIn.read(reinterpret_cast < char * > (&joints[i].m_Name), sizeof(char) * 32);
				//then the parent name, again 32 chars
				fileIn.read(reinterpret_cast < char * > (&joints[i].m_ParentName), sizeof(char) * 32);

				//read in the rotation then the position, these are 3 floats, we read directly into a slVec3
				fileIn.read(reinterpret_cast < char * > (&joints[i].m_Rotation), sizeof(float) * 3);
				fileIn.read(reinterpret_cast < char * > (&joints[i].m_Position), sizeof(float) * 3);

				//now the number of rotation keyframes, followed by the number of position keyframes
				fileIn.read(reinterpret_cast < char * > (&joints[i].m_NumKeyframesRotation), sizeof(word));
				fileIn.read(reinterpret_cast < char * > (&joints[i].m_NumKeyframesPosition), sizeof(word));

				//Create space for the key frames
				joints[i].m_KeyframesRotation.resize(joints[i].m_NumKeyframesRotation);
				joints[i].m_KeyframesPosition.resize(joints[i].m_NumKeyframesPosition);

				slMat4Identity(&joints[i].m_Relative); //Initialize the matrices!
				slMat4Identity(&joints[i].m_Absolute);
				slMat4Identity(&joints[i].m_Final);
				//read in the data
				for (int j = 0; j < joints[i].m_NumKeyframesRotation; ++j) {
					fileIn.read(reinterpret_cast < char * > (&joints[i].m_KeyframesRotation[j]),
								sizeof(MS3DKeyFrameRotation));
					//	std::cout << m_Joints[i].m_KeyframesRotation[j].m_Rotation.m_X << ", " << m_Joints[i].m_KeyframesRotation[j].m_Rotation.m_Y << ", " << m_Joints[i].m_KeyframesRotation[j].m_Rotation.m_Z << std::endl;
				}
				for (int j = 0; j < joints[i].m_NumKeyframesPosition; ++j) {
					fileIn.read(reinterpret_cast < char * > (&joints[i].m_KeyframesPosition[j]),
								sizeof(MS3DKeyFramePosition));
				}
			}
			return true;
		}


        bool MS3D::LoadModelFile(const string& filename) {
			vector<MS3DVertex> vertices;
			vector<MS3DGroup> groups;
			vector<MS3DTriangle> triangles;
			vector<MS3DMaterial> materials;

			ifstream fileIn(filename.c_str(), std::ios::binary);

			if (!fileIn.is_open()) {
				SetErrorString("Could not open model file: " + filename);
				return false;
			}

			if (!CheckFileHeaderIsValid(fileIn)) {
				SetErrorString("Model file is not valid: " + filename);
				return false;
			}

			word numVertices = 0, numTriangles = 0, numGroups = 0, numMaterials = 0, numJoints = 0;

			//read the vertex count
			fileIn.read(reinterpret_cast < char * > (&numVertices), sizeof(word));
			ReadVertices(fileIn, vertices, numVertices); //Read the vertices

			fileIn.read(reinterpret_cast < char * > (&numTriangles), sizeof(word));
			ReadTriangles(fileIn, triangles, numTriangles);

			//Read the group count
			fileIn.read(reinterpret_cast < char * > (&numGroups), sizeof(word));
			ReadGroups(fileIn, groups, numGroups); //Read the groups

			fileIn.read(reinterpret_cast < char * > (&numMaterials), sizeof(word));
			ReadMaterials(fileIn, materials, numMaterials);

			float animationFPS;
			float currentTime;
			int totalFrames;

			fileIn.read(reinterpret_cast < char * > (&animationFPS), sizeof(float));
			fileIn.read(reinterpret_cast < char * > (&currentTime), sizeof(float));
			fileIn.read(reinterpret_cast < char * > (&totalFrames), sizeof(int));


			SetFPS(animationFPS); //So we can animate the model

			std::cout << "FPS: " << animationFPS << std::endl;

			fileIn.read(reinterpret_cast < char * > (&numJoints), sizeof(word));
			ReadJoints(fileIn, m_Joints, numJoints);
			std::cout << "Joints: " << numJoints << std::endl;

			fileIn.close();

			SetupJoints(vertices, triangles);
			StoreInInternalFormat(vertices, groups, triangles, materials);
			//Update(0.0); //Build the interpolated frame

            return true;
        }

        bool MS3D::StoreInInternalFormat(const vector<MS3DVertex>& vertices, 
										 const vector<MS3DGroup>& groups, 
										 const vector<MS3DTriangle>& triangles, 
										 const vector<MS3DMaterial>& materials)
        {
 			GetFrames().resize(1); //Set up one frame
 			
 			int numTriangleIndices = 0;
 			
 			unsigned int idx = 0;
 			
			for (vector<MS3DGroup>::const_iterator i = groups.begin(); i != groups.end(); ++i) {
				unsigned int materialID = (*i).m_MaterialIndex;
				TriangleGroup& g = GetFrame(0).GetGroup(materialID);
				
				numTriangleIndices += (*i).m_TriangleIndices.size() * 3;
				
				GetFrame(0).GetVertices().resize(numTriangleIndices);
				GetFrame(0).GetBoneIDs().resize(numTriangleIndices);
				GetFrame(0).GetNormals().resize(numTriangleIndices);
				GetFrame(0).GetTexCoords().resize(numTriangleIndices);
				
				for (vector<word>::const_iterator j = (*i).m_TriangleIndices.begin(); 
							j != (*i).m_TriangleIndices.end(); ++j) {
			
					for (int k = 0; k < 3; ++k, ++idx) {
						GetFrame(0).GetVertexPtr()[idx] = vertices[triangles[(*j)].m_Index[k]].m_Pos;
						GetFrame(0).GetBoneIDs()[idx] = vertices[triangles[(*j)].m_Index[k]].m_BoneID;
						GetFrame(0).GetNormalPtr()[idx] = triangles[(*j)].m_Normals[k];
						GetFrame(0).GetTexCoordPtr()[idx] = slVec2(triangles[(*j)].m_S[k], triangles[(*j)].m_T[k]);
						
						g.GetIndices().push_back(idx);
					}
				}
			}
 			
			GetMaterials().resize(materials.size());
			int i = 0;
			for (vector<MS3DMaterial>::const_iterator m = materials.begin(); m != materials.end(); ++m, ++i) {
				slVec4Assign(&GetMaterials()[i].m_Ambient, &(*m).m_Ambient);
				slVec4Assign(&GetMaterials()[i].m_Diffuse, &(*m).m_Diffuse);
				slVec4Assign(&GetMaterials()[i].m_Emissive, &(*m).m_Emissive);
				slVec4Assign(&GetMaterials()[i].m_Specular, &(*m).m_Specular);
				GetMaterials()[i].m_Shininess = (*m).m_Shininess;
				GetMaterials()[i].m_Transparency = (*m).m_Transparency;
				GetMaterials()[i].m_TextureFile1 = string((*m).m_Texture);
				GetMaterials()[i].m_TextureFile2 = string((*m).m_Alphamap);
			}
			
			return true;
        }


		int MS3D::FindBoneByName(const string& name) {
			if (name.size() == 0)
				return -1;

			int i = 0;
			typedef vector<MS3DJoint>::iterator JointIter;
			for (JointIter it = m_Joints.begin(); it != m_Joints.end(); ++it, ++i) {
				if (name == string((*it).m_Name) ) {
					return i;
				}
			}

			return -1;
		}

		void MS3D::SetupJoints(vector<MS3DVertex>& vertices, vector<MS3DTriangle>& triangles) {
			if (!m_Joints.empty()) {
				//Loop through the joints
				for (vector<MS3DJoint>::iterator j = m_Joints.begin(); j != m_Joints.end(); ++j) {
					MS3DJoint* joint = &(*j);

					//Build the rotation from the rotation vector
					slMat4RotationPitchYawRoll(&joint->m_Relative, joint->m_Rotation.m_X,
																	joint->m_Rotation.m_Y,
																	 joint->m_Rotation.m_Z);
					//Set the translation of the relative matrix
					joint->m_Relative.m_Mat[12] = joint->m_Position.m_X;
					joint->m_Relative.m_Mat[13] = joint->m_Position.m_Y;
					joint->m_Relative.m_Mat[14] = joint->m_Position.m_Z;

					int parentID = FindBoneByName(joint->m_ParentName);

					if (parentID != -1) {
						slMat4 temp; //Temp matrix
						slMat4Identity(&temp);
						//Multiply the absolute matrix by this joints relative matrix and assign it to this joints absolute
						slMat4Multiply(&temp, &m_Joints[parentID].m_Absolute,  &(*j).m_Relative);
						slMat4Assign(&(*j).m_Absolute, &temp);
					} else {
						//Copy the relative matrix to the absolute as we have no parent
						slMat4Assign(&(*j).m_Absolute, &(*j).m_Relative);
					}
				}

				for (vector<MS3DVertex>::iterator v = vertices.begin(); v != vertices.end(); ++v) {
					if ((*v).m_BoneID != -1) {
						assert((*v).m_BoneID < (int)m_Joints.size());
						assert((*v).m_BoneID >= 0);

						slVec3InverseTransform(&(*v).m_Pos, &(*v).m_Pos, &m_Joints[(*v).m_BoneID].m_Absolute);
					}
				}

				typedef vector<MS3DTriangle>::iterator TriangleIter;
				for (TriangleIter it = triangles.begin(); it != triangles.end(); ++it) {
					for (int k = 0; k < 3; ++k)	{
						int boneID = vertices[(*it).m_Index[k]].m_BoneID;
						if (boneID != -1) {
							slVec3InverseTransformNormal(&(*it).m_Normals[k], &(*it).m_Normals[k], &m_Joints[boneID].m_Absolute);
						}
					}
				}
			}
		}

        void MS3D::BasicGLRender() const {
			glEnableClientState(GL_VERTEX_ARRAY);
			glEnableClientState(GL_NORMAL_ARRAY);
			glEnableClientState(GL_TEXTURE_COORD_ARRAY);

				//Use the interpolated vertices!
				glVertexPointer(3, GL_FLOAT, 0, GetInterpolatedFrame().GetVertexPtr());
				glNormalPointer(GL_FLOAT, 0, GetInterpolatedFrame().GetNormalPtr());
				glTexCoordPointer(2, GL_FLOAT, 0, GetFrame(0).GetTexCoordPtr());
				//Draw all of the groups

				for (map<unsigned int, TriangleGroup>::const_iterator it = GetFrame(0).GetGroups().begin();
						it != GetFrame(0).GetGroups().end(); ++it) {

					glDrawElements(GL_TRIANGLES, it->second.GetIndices().size(), GL_UNSIGNED_INT, it->second.GetIndexPtr());
				}

			glDisableClientState(GL_TEXTURE_COORD_ARRAY);
			glDisableClientState(GL_NORMAL_ARRAY);
			glDisableClientState(GL_VERTEX_ARRAY);
        }


		void MS3D::RenderGroup(unsigned int groupID) const {
			glEnableClientState(GL_VERTEX_ARRAY);
			glEnableClientState(GL_NORMAL_ARRAY);
				glVertexPointer(3, GL_FLOAT, 0, GetInterpolatedFrame().GetVertexPtr());
				glNormalPointer(GL_FLOAT, 0, GetInterpolatedFrame().GetNormalPtr());
				glDrawElements(GL_TRIANGLES, GetFrame(0).GetGroup(groupID).GetIndices().size(),
										GL_UNSIGNED_INT, GetFrame(0).GetGroup(groupID).GetIndexPtr());

			glDisableClientState(GL_NORMAL_ARRAY);
			glDisableClientState(GL_VERTEX_ARRAY);

		}

		void MS3D::ApplyTransformations() {

			GetInterpolatedFrame().GetVertices().resize(GetFrame(0).GetVertices().size());
			GetInterpolatedFrame().GetNormals().resize(GetFrame(0).GetNormals().size());
			int i = 0;
			typedef vector<slVec3>::const_iterator VertexIter;
			VertexIter v = GetFrame(0).GetVertices().begin();
			VertexIter n = GetFrame(0).GetNormals().begin();

			for (; v != GetFrame(0).GetVertices().end(); ++v, ++n, ++i) {
				GetInterpolatedFrame().GetVertexPtr()[i] = (*v);
				GetInterpolatedFrame().GetNormalPtr()[i] = (*n);

				if (GetFrame(0).GetBoneID(i) == -1)
					continue;
				else {
					const slMat4 *mat = &m_Joints[GetFrame(0).GetBoneID(i)].m_Final;
					slVec3Transform(&GetInterpolatedFrame().GetVertexPtr()[i], &GetInterpolatedFrame().GetVertexPtr()[i], mat);
					slVec3TransformNormal(&GetInterpolatedFrame().GetNormalPtr()[i], &GetInterpolatedFrame().GetNormalPtr()[i], mat);
					//slTransformVector(mat, &m_RenderableVertices.GetVertices()[i].m_Position);
				}

			}
		}

		float MS3D::CalculateInterpolation(float currentTime, float prevTime, float nextTime) {
			if (currentTime < prevTime)
				return prevTime;

			if (currentTime > nextTime)
				return nextTime;

			return (currentTime - prevTime) / (nextTime - prevTime);
		}

		void MS3D::SetFrameTime(float time) {
			if (m_Joints.empty())
				return;

            typedef vector<MS3DJoint>::iterator JointIter;
            for (JointIter it = m_Joints.begin(); it != m_Joints.end(); ++it) {
				if ((*it).m_NumKeyframesPosition == 0 && (*it).m_NumKeyframesRotation == 0) {
					slMat4Assign(&(*it).m_Final, &(*it).m_Absolute);
					continue;
				}

				std::pair<MS3DKeyFramePosition*, MS3DKeyFramePosition*> posKeys =
					FindNearestKeyFrames <MS3DKeyFramePosition> (time, (*it).m_KeyframesPosition);

				std::pair<MS3DKeyFrameRotation*, MS3DKeyFrameRotation*> rotKeys =
					FindNearestKeyFrames <MS3DKeyFrameRotation> (time, (*it).m_KeyframesRotation);

				//Gotta love templates ;)

                //Calculate the interpolation we are at between the 2 frames
                float transinterp = CalculateInterpolation(time, posKeys.first->m_Time, posKeys.second->m_Time);
                float rotinterp = CalculateInterpolation(time, rotKeys.first->m_Time, rotKeys.second->m_Time);

				slVec3 translation, rotation;

				translation.m_X = posKeys.first->m_Position.m_X + (posKeys.second->m_Position.m_X - posKeys.first->m_Position.m_X) * transinterp;
				translation.m_Y = posKeys.first->m_Position.m_Y + (posKeys.second->m_Position.m_Y - posKeys.first->m_Position.m_Y) * transinterp;
				translation.m_Z = posKeys.first->m_Position.m_Z + (posKeys.second->m_Position.m_Z - posKeys.first->m_Position.m_Z) * transinterp;

                rotation.m_X = rotKeys.first->m_Rotation.m_X + (rotKeys.second->m_Rotation.m_X - rotKeys.first->m_Rotation.m_X) * rotinterp;
                rotation.m_Y = rotKeys.first->m_Rotation.m_Y + (rotKeys.second->m_Rotation.m_Y - rotKeys.first->m_Rotation.m_Y) * rotinterp;
                rotation.m_Z = rotKeys.first->m_Rotation.m_Z + (rotKeys.second->m_Rotation.m_Z - rotKeys.first->m_Rotation.m_Z) * rotinterp;


                slMat4 relativeFinal, transform;
                slMat4Identity(&transform);
                slMat4Identity(&relativeFinal);

				//Build the interpolated matrix
				//Build the rotation from the rotation vector
				slMat4 rotx, roty, rotz;

				slMat4RotationX(&rotx, rotation.m_X);
				slMat4RotationY(&roty, rotation.m_Y);
				slMat4RotationZ(&rotz, rotation.m_Z);

				slMat4Multiply(&transform, &rotx, &roty);
				slMat4Multiply(&transform, &transform, &rotz);

				//slMat4RotationYawPitchRoll(&transform, rotation.m_X,
					//									rotation.m_Y,
						//								 rotation.m_Z);


				//slPrintMatrix(&transform);

				//Set the translation of the relative matrix
				transform.m_Mat[12] = translation.m_X;
				transform.m_Mat[13] = translation.m_Y;
				transform.m_Mat[14] = translation.m_Z;

                //Multiply this with the relative matrix and assign it to relativeFinal
                slMat4Multiply(&relativeFinal, &(*it).m_Relative, &transform);

                int parentID = FindBoneByName((*it).m_ParentName);
                if (parentID == -1) {
					//Final is relative final
					slMat4Assign(&(*it).m_Final, &relativeFinal);

					continue; //Go to next loop
                }

				//Multiply the parents final matrix with relativeFinal and assign to this joint's final matrix
                slMat4Multiply(&(*it).m_Final, &m_Joints[parentID].m_Final, &relativeFinal);
            }
		}

        void MS3D::Update(float dT) {
        	        	
			if (GetAnimations().empty()) {
				SetFrameTime(GetAnimTime());
				ApplyTransformations();
				return;
			}

			IncrementCurrentAnimationTime(dT);

			if (GetAnimTime() >= GetAnimEndTime()) {
				MoveNextAnimation();
			}

			SetFrameTime(GetAnimTime());
			ApplyTransformations();
        }
        
        //=======================================================
        // API FUNCTIONS
        
        void MS3D::slBasicRender() const {
        	BasicGLRender();
        }
        
        void MS3D::slRenderSubset(unsigned int i) const {
			RenderGroup(i);
        }
        
    }
}
